package com.suray.system.run.random.code;

import com.suray.basic.wcs.utils.WcsFuncUtil;
import com.suray.system.run.random.code.util.NavNode;
import com.suray.system.run.random.code.util.NodePoint;

import java.util.ArrayList;
import java.util.List;

public class LadderNavigateCore {

    private NavNode[][] map;

    private NodePoint startPoint;

    /**
     *
     * @param nodes：[x][y]->y为货位方向
     * @param startPoint
     */
    public LadderNavigateCore(NavNode[][] nodes, NodePoint startPoint) {
        if (startPoint == null) {
            throw new RuntimeException("无法计算小车的起始跑码位置，请重新选择跑码区域或手动把小车开到起始点");
        }

        this.map = nodes;
        this.startPoint = startPoint;
    }

    /**
     * 解析导航路径
     * @return
     */
    public List<NavNode> analyzePath() {
        List<NavNode> path = new ArrayList<>();
        List<NavNode> openNodes = new ArrayList<>();
        openNodes.add(getNode(startPoint));
        while (openNodes.size() > 0) {
            List<NavNode> nearNodes = new ArrayList<>();
            NavNode node = openNodes.get(openNodes.size() - 1);
            openNodes.remove(node);
            NodePoint nodePoint = node.getNodePoint();
            checkIfNeedPreNode(path, node);
            path.add(node);

            node.setRunEnd(true);
            if (checkRunEnd()) {
                break;
            }

            if (node.getUp() && nodePoint.upPoint() != null) {
                addNearNodes(nearNodes, getNode(nodePoint.upPoint()), node, openNodes);
            }
            if (node.getDown() && nodePoint.downPoint() != null) {
                addNearNodes(nearNodes, getNode(nodePoint.downPoint()), node, openNodes);
            }
            if (node.getLeft() && nodePoint.leftPoint() != null) {
                addNearNodes(nearNodes, getNode(nodePoint.leftPoint()), node, openNodes);
            }
            if (node.getRight() && nodePoint.rightPoint() != null) {
                addNearNodes(nearNodes, getNode(nodePoint.rightPoint()), node, openNodes);
            }

            nearNodes.sort((o1, o2) -> o2.getLadder() - o1.getLadder());

            for (NavNode nearNode : nearNodes) {
                if (openNodes.contains(nearNode)) {
                    openNodes.remove(nearNode);
                }

                openNodes.add(nearNode);
//                System.out.println("--->" + node + "---" + nearNode);
            }
        }

        System.out.println(WcsFuncUtil.listToString(path));

        return path;
    }

    /**
     * 如果路径不连续，下段路径的起点添加前置点，小车先行驶到前置点，以前置点为起点，行驶下段路径
     * @param path
     * @param node
     */
    private void checkIfNeedPreNode(List<NavNode> path, NavNode node) {
        if (path.size() < 1) {
            return ;
        }

        NavNode lastNode = path.get(path.size() - 1);
        if (!lastNode.near(node)) {
            path.add(node.getPreNode());
        }
    }

    /**
     * 添加点位附近的点
     * @param nearNodes
     * @param node
     * @param preNode
     * @param openNodes
     */
    private void addNearNodes(List<NavNode> nearNodes, NavNode node, NavNode preNode, List<NavNode> openNodes) {
        if (node != null) {
            if (!node.isRunEnd()) {
                nearNodes.add(node);
                node.setPreNode(preNode);
            } else {
                for (NavNode openNode : openNodes) {
                    //这里只添加已经跑了，但是没跑完的楼层
                    if (openNode.getPreNode().equals(node)
                            && openNode.getLadder() <= preNode.getLadder()) {
                        nearNodes.add(openNode);
                        break;
                    }
                }
            }
        }
    }

    /**
     * 是否跑完所有的位置
     * @return
     */
    private boolean checkRunEnd() {
        for (NavNode[] navNodes : map) {
            for (NavNode navNode : navNodes) {
                if (navNode != null && !navNode.isRunEnd()) {
                    return false;
                }
            }
        }

        return true;
    }

    private NavNode getNode(NodePoint point) {
        if (point.getX() < map.length && point.getY() < map[0].length) {
            NavNode node = map[point.getX()][point.getY()];
            node.setNodePoint(point);
            return map[point.getX()][point.getY()];
        }

        return null;
    }
}
